#include "Util.h"
using namespace sf;

void normalize(Vector2f& vector) {
	float size = length(vector);
	vector /= size;
}

Vector2f calculateOrthogonalVector(Vector2f vector) {
	normalize(vector);
	Vector2f orthogonal;
	orthogonal.x = vector.y;
	orthogonal.y = -vector.x;
	return orthogonal;
}

bool isNullVector(const Vector2f& vector) {
	return vector.x == 0 && vector.y == 0;
}

float length(Vector2f v) {
	return sqrt(v.x * v.x + v.y * v.y);
}
float getSmallAngle(Vector2f u, Vector2f v) {
	normalize(u);
	normalize(v);
	return acos(scalarProduct(u, v));
}

float scalarProduct(Vector2f u, Vector2f v) {
	return u.x * v.x + u.y * v.y;
}

void drawRectInImage(Image& image, Shape& shape) {
	Shape triangle1, triangle2, triangle3, triangle4;
	Color color = shape.GetColor();
	triangle1.SetColor(color);
	triangle2.SetColor(color);
	//	triangle3.SetColor(color);
	//	triangle4.SetColor(color);
	triangle1.AddPoint(shape.GetPointPosition(0));
	triangle1.AddPoint(shape.GetPointPosition(1));
	triangle1.AddPoint(shape.GetPointPosition(2));
	triangle2.AddPoint(shape.GetPointPosition(0));
	triangle2.AddPoint(shape.GetPointPosition(3));
	triangle2.AddPoint(shape.GetPointPosition(2));
	//	triangle3.AddPoint(shape.GetPointPosition(0));
	//	triangle3.AddPoint(shape.GetPointPosition(1));
	//	triangle3.AddPoint(shape.GetPointPosition(3));
	//	triangle4.AddPoint(shape.GetPointPosition(2));
	//	triangle4.AddPoint(shape.GetPointPosition(3));
	//	triangle4.AddPoint(shape.GetPointPosition(1));
	drawTriangleInImage(image, triangle1);
	drawTriangleInImage(image, triangle2);
	//	drawTriangleInImage(image, triangle3);
	//	drawTriangleInImage(image, triangle4);
}

void drawTriangleInImage(Image& image, Shape& shape) {
	Vector2f topLeft = shape.GetPointPosition(0);
	Vector2f bottomRight = shape.GetPointPosition(0);
	for (int i = 1; i < 3; i++) {
		Vector2f point = shape.GetPointPosition(i);
		if (point.x < topLeft.x)
			topLeft.x = point.x;
		if (point.y < topLeft.y)
			topLeft.y = point.y;
		if (point.x > bottomRight.x)
			bottomRight.x = point.x;
		if (point.x > bottomRight.y)
			bottomRight.y = point.y;
	}
	if (topLeft.x == bottomRight.x || topLeft.y == bottomRight.y)
		return;
	Vector2f p = shape.GetPointPosition(0);
	Vector2f u = shape.GetPointPosition(1) - p;
	Vector2f v = shape.GetPointPosition(2) - p;
	for (int x = topLeft.x; x <= bottomRight.x; x++) {
		for (int y = topLeft.y; y <= bottomRight.y; y++) {
			float j = calcJ(p, u, v, x, y);
			float i = calcI(p, u, v, x, y, j);
			if (i + j <= 1 && i + j >= 0) {
				image.SetPixel(x, y, shape.GetColor());
			} else {
			}
		}
	}
}

float calcJ(Vector2f p, Vector2f u, Vector2f v, float x, float y) {
	return (u.x * y - u.x * p.y - u.y * x + u.y * p.x)
			/ (v.y * u.x - v.x * u.y);
}

float calcI(Vector2f p, Vector2f u, Vector2f v, float x, float y, float j) {
	return (x - p.x - j * v.x) / u.x;
}

bool intersect(Vector2f p1, Vector2f v1, Vector2f p2, Vector2f v2) {
	float j = calcJ(p1, v1, -v2, p2.x, p2.y);
	float i = calcI(p1, v1, -v2, p2.x, p2.y, j);
	return (i >= 0 && i < 1) && (j >= 0 && j <= 1);
}

float calcA2(Vector2f p1, Vector2f v1, Vector2f p2, Vector2f v2) {
	return (p1.x + p2.y * v1.x / v1.y - p1.y * v1.x / v1.y - p2.x) / (v2.x
			- v2.y * v1.x / v1.y);
}

float calcA1(Vector2f p1, Vector2f v1, Vector2f p2, Vector2f v2, float a2) {
	return (p2.y + a2 * v2.y - p1.y) / v1.y;
}
